#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     HTIRS2,         sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     motorLEFT,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNone, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorRIGHT,    tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/HTIRS2-driver.h"
int DirPwr(int Dir);
int SelectDir();
void moveForward(int distanceInInches);
void moveBackward(int distanceInInches);
void turnLeft(int angleInDegrees);
void turnRight(int angleInDegrees);
int ReadSignalStrength();
int Max(int num1, int num2);
int GetDirection();

int _dirAC = 0;
int _Dir;
int acS1, acS2, acS3, acS4, acS5 = 0;
tHTIRS2DSPMode _mode = DSP_1200;
int direction = 0;
int signalStrength = 0;
int distanceTravelled = 0;
int MAX_SIGNAL_STRENGTH = 140;

task main
{
  //_Dir = SelectDir();
  // go straight down the ramp
  moveForward(36); // TODO: measure the distance and replace with 36
  // make turn towards IR sensor
  turnRight(45);

  // see whether we can get the direction and some signal strength
  // also check whether we are not travelling beyond 10 feet
  while (ReadSignalStrength() < 10 && distanceTravelled < 120)
  {
    moveForward(3);
    distanceTravelled += 3; // keep track of how much we are travelling
  }

  if (signalStrength > MAX_SIGNAL_STRENGTH || distanceTravelled > 119)
  {
    return;
  }

  // if we get signal strength turn the robot
  // till we get direction 5
  direction = GetDirection();
  if (direction == 0)
  {
    return; // after all this if we do not get the direction then better quit
  }

  if (direction < 5)
  {
    // if direction is less than 5 then robot needs to turn left
    while (GetDirection() < 5)
    {
      turnLeft(3);
    }
  }
  else if (direction > 5)
  {
    // if direction is greater than 5 then robot needs to turn right
    while (GetDirection() > 5)
    {
      turnRight(3);
    }
  }

  // TODO: hopefully we have direction as 5 now make sure

  // move the robot while direction is 5 and signal strength increasing and
  // distance travelled is less than 6 feet
  while (ReadSignalStrength() < MAX_SIGNAL_STRENGTH && distanceTravelled < 100)
  {
    // TODO: We may have to move motors continuosly and in that case add the motor
    // movement code here
    moveForward(4); // move 4 inches at a time
    distanceTravelled += 4;
  }
}

int ReadSignalStrength()
{
  HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 );
  int ii = Max(acS1, acS2);
  ii = Max(ii, acS3);
  ii = Max(ii, acS4);
  ii = Max(ii, acS5);
  return ii;
}

int Max(int num1, int num2)
{
  return num1 > num2 ? num1 : num2;
}

int DirPwr(int Dir)
{
  HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 );
  switch(Dir)
  {
  case 1:
    return acS1;
    break;
  case 2:
    return ((acS1 + acS2)/2);
    break;
  case 3:
    return acS2;
    break;
  case 4:
    return ((acS2 + acS3)/2);
    break;
  case 5:
    return acS3;
    break;
  case 6:
    return ((acS3 + acS4)/2);
    break;
  case 7:
    return acS4;
    break;
  case 8:
    return ((acS4 + acS5)/2);
    break;
  case 9:
    return acS5;
    break;
  default:
    return 0;
    break;
  }
}

void moveForward(int distanceInInches)
{
  // encoder gives 1440 pulses per revolution and we are using 4 inch wheels
  float encoderTicks = 1440 * distanceInInches / (2 * 3.14 * 4);
  nMotorEncoder[motorLEFT] = 0;
  nMotorEncoder[motorRIGHT] = 0;

  // move the motor until encoder readings are less than expected
  while(nMotorEncoder[motorLEFT] < encoderTicks)
  {
    motor[motorLEFT] = 100;
    motor[motorRIGHT] = 100;
  }
  // reset motor power
  motor[motorLEFT] = 0;
  motor[motorRIGHT] = 0;
}

void moveBackward(int distanceInInches)
{
  // encoder gives 1440 pulses per revolution and we are using 4 inch wheels
  float encoderTicks = 1440 * distanceInInches / (2 * 3.14 * 4);
  nMotorEncoder[motorLEFT] = 0;
  nMotorEncoder[motorRIGHT] = 0;

  // move the motor until encoder readings are less than expected
  while(nMotorEncoder[motorLEFT] < encoderTicks)
  {
    motor[motorLEFT] = -100;
    motor[motorRIGHT] = -100;
  }
  // reset motor power
  motor[motorLEFT] = 0;
  motor[motorRIGHT] = 0;
}

void turnLeft(int angleInDegrees)
{
  // TODO: Verify this hypothesis
  // half a turn of rotation of both wheels in opposite direction will result in
  // full rotation of robot
  float encoderTicks = angleInDegrees * 720 / 360;

  nMotorEncoder[motorLEFT] = 0;
  nMotorEncoder[motorRIGHT] = 0;

  // move the motor until encoder readings are less than expected
  while(nMotorEncoder[motorLEFT] < encoderTicks)
  {
    motor[motorLEFT] = 80;
    motor[motorRIGHT] = -80;
  }
  // reset motor power
  motor[motorLEFT] = 0;
  motor[motorRIGHT] = 0;
}

void turnRight(int angleInDegrees)
{
  // half a turn of rotation of both wheels in opposite direction will result in
  // full rotation of robot
  float encoderTicks = angleInDegrees * 720 / 360;

  nMotorEncoder[motorLEFT] = 0;
  nMotorEncoder[motorRIGHT] = 0;

  // move the motor until encoder readings are less than expected
  while(nMotorEncoder[motorLEFT] < encoderTicks)
  {
    motor[motorLEFT] = -80;
    motor[motorRIGHT] = 80;
  }
  // reset motor power
  motor[motorLEFT] = 0;
  motor[motorRIGHT] = 0;
}

int GetDirection()
{
  int ii = HTIRS2readACDir(HTIRS2);
  wait10Msec(1);
  ii += HTIRS2readACDir(HTIRS2);
  wait10Msec(1);
  ii += HTIRS2readACDir(HTIRS2);

  return ii / 3;
}

int SelectDir()
{
  int nIndex = 1;
  while(true)
  {
    if(nNxtButtonPressed == kLeftButton)
    {
      if(nIndex == 1)
        nIndex = 9;
      else
        nIndex--;
      wait1Msec(300);
      PlaySoundFile("! Click.rso");
    }
    if(nNxtButtonPressed == kRightButton)
    {
      if(nIndex == 9)
        nIndex = 1;
      else
        nIndex++;
      wait1Msec(300);
      PlaySoundFile("! Click.rso");
    }
    if(nNxtButtonPressed == kEnterButton)
    {
      wait1Msec(300);
      PlaySoundFile("! Click.rso");
      break;
    }
    nxtDisplayStringAt(0, 31, "Direction?");
    nxtDisplayBigStringAt(0, 15, "%d", nIndex);
    wait1Msec(100);
  }
  eraseDisplay();
  nxtDisplayStringAt(0, 15, "Searching...");
  return(nIndex);
}
